package org.opentcs.kcvehicle.communication.kc.udp;

import java.nio.ByteOrder;
import java.util.ArrayList;
import java.util.List;
import org.opentcs.kcvehicle.communication.kc.common.byteutils.ByteUtils;
import org.opentcs.kcvehicle.communication.kc.udp.Service.ActImmediately;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.AgvEvent;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.af.QueryRobotStatusRsp;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.b1.SubscribeInfo;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.b1.SubscribeParam;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.navigation.*;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.read.ReadParam;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.read.ReadStrValue;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.read.ReadValueMember;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.write.WriteParam;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.write.WriteStrValue;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.write.WriteValueMember;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.x14.RobotSetPosition;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kcvehicle.communication.kc.udp.io.UDPClient;

/**
 * AGV启动：
 * 0xAF(查询机器人状态) 👌
 * 0xB1(下发订阅信令) 👌
 * 初始化：
 * 0x03(切换手自动) 👌
 * 0x14(手动定位) 👌
 * 0x17(查询机器人运行状态) 👌
 * 0x1F(确认初始位置) 👌
 * 运行：
 * 0xAE(导航控制导航点控制) 👌
 */
public class KCCommandDemo {
  public static void main(String[] args)
      throws Exception {
        {
            //0xAF(查询机器人状态)
            AgvEvent agvEvent = queryStatus();
            printInfo(agvEvent);
            RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
            if(rcv.isOk()){
              System.out.println();
              System.out.println("received transationId : "+ "isok:"+rcv.isOk());
              for (byte b:rcv.getValue()){
                System.out.print(byteToHex(b)+" ");
              }
              System.out.println();
              System.out.println("---------------------");
              for (byte c:rcv.getDataBytes()){
                System.out.print(byteToHex(c)+" ");
              }
                QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes());
            }else {
                System.out.println();
                System.out.println("received transationId : "+ "isok:"+rcv.isOk());
            }
        }

//        {
//            //0xB0(查询载货状态)
//            AgvEvent agvEvent = checkCargoStatus();
//            printInfo(agvEvent);
//            RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
//            if(rcv.isOk()){
//                QueryCargoStatusRsp queryCargoStatusRsp = new QueryCargoStatusRsp(rcv.getDataBytes());
//                System.out.println();
//                System.out.println("received transationId : "+ "isok:"+rcv.isOk());
//                for (byte b:rcv.getValue()){
//                    System.out.print(byteToHex(b)+" ");
//                }
//            }else {
//                System.out.println();
//                System.out.println("received transationId : "+ "isok:"+rcv.isOk());
//            }
//        }

//        {
//            //0xB1(订阅信息)
//            AgvEvent agvEvent = issueSubscribe();
//            printInfo(agvEvent);
//            //todo 订阅参数构建完毕 去写 回调部分
//            RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
//            if(rcv.isOk()){
//                System.out.println();
//                System.out.println("received transationId : "+ "isok:"+rcv.isOk());
//                for (byte b:rcv.getDataBytes()) {
//                  System.out.print(byteToHex(b)+" ");
//                }
//
//                SubscribeRsp subscribeRsp = new SubscribeRsp(rcv.getDataBytes());
//                if(subscribeRsp.isOk()){
//                    //...
//                }else {
//                    //...
//                }
//            }else {
//                System.out.println();
//                System.out.println("received transationId : "+ "isok:"+rcv.isOk());
//            }
//        }

//        {
//            //0x02(read操作)
//            AgvEvent agvEvent = readValue();
//            printInfo(agvEvent);
//            RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
//            if(rcv.isOk()){
//                System.out.println();
//                System.out.println("received transationId : "+ "isok:"+rcv.isOk());
//                printInfo(rcv);
//                ReadRsp readRsp = new ReadRsp(rcv.getDataBytes());
//                if(readRsp.isOk()){
//                    //get and parse value
//                    System.out.println("read ok");
//                }else {
//                    System.out.println("read failed");
//                }
//            }else {
//                System.out.println();
//                System.out.println("received transationId : "+ "isok:"+rcv.isOk());
//            }
//        }

//        {
//            //0x03(切换手自动)
//            AgvEvent agvEvent = writeValue();
//            printInfo(agvEvent);
//            RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
//            if(rcv.isOk()){
//                System.out.println();
//                System.out.println("received "+ "isok:"+rcv.isOk()+" dataBytes:");
//                printInfo(rcv);
//                if(rcv.isOk()){
//                    //get and parse value
//                    System.out.println("write ok");
//                }else {
//                    System.out.println("write failed");
//                }
//            }else {
//                System.out.println();
//                System.out.println("received transationId : "+ "isok:"+rcv.isOk());
//            }
//        }


//        {
//            //0x14(手动定位)
//            AgvEvent agvEvent = manualLocation();
//            printInfo(agvEvent);
//            RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
//            if(rcv.isOk()){
//                System.out.println();
//                System.out.println("received "+ "isok:"+rcv.isOk()+" dataBytes:");
//                printInfo(rcv);
//                if(rcv.isOk()){
//                    //get and parse value
//                    System.out.println("0x14 ok");
//                }else {
//                    System.out.println("0x14 failed");
//                }
//            }else {
//                System.out.println();
//                System.out.println("received transationId : "+ "isok:"+rcv.isOk());
//            }
//        }


//        {
//            //0x17(查询机器人运行状态)
//            AgvEvent agvEvent = queryRobotRunStatus();
//            printInfo(agvEvent);
//            RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
//            if(rcv.isOk()){
//              System.out.println();
//              System.out.println("received transationId : "+ "isok:"+rcv.isOk());
//              for (byte b:rcv.getValue()){
//                System.out.print(byteToHex(b)+" ");
//              }
//              System.out.println();
//              System.out.println("print databytes:-----=======");
//              for (byte c:rcv.getDataBytes()){
//                System.out.print(byteToHex(c)+" ");
//              }
//                //QueryCargoStatusRsp queryCargoStatusRsp = new QueryCargoStatusRsp(rcv.getDataBytes());
//                QueryRobotRunStatusRsp queryRobotRunStatusRsp = new QueryRobotRunStatusRsp(rcv.getDataBytes());
//                System.out.println(queryRobotRunStatusRsp.toString());
//
//            }else {
//                System.out.println();
//                System.out.println("received transationId : "+ "isok:"+rcv.isOk());
//            }
//        }

//    {
//      //0x1F(确认初始位置)
//      AgvEvent agvEvent = confirmInitialPosition();
//      printInfo(agvEvent);
//      RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
//      if (rcv.isOk()) {
//        System.out.println("0x1F ok");
//      }
//      else {
//        System.out.println();
//        System.out.println("0x1F fail");
//        System.out.println("received transationId : " + "isok:" + rcv.isOk());
//      }
//    }

//        {
//            //0xAE(导航控制导航点控制)
//            AgvEvent agvEvent = navigationControl();
//            printInfo(agvEvent);
//            RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
//            if(rcv.isOk()){
//                System.out.println("0xAE ok");
//            }else {
//                System.out.println();
//                System.out.println("0xAE fail");
//                System.out.println("received transationId : "+ "isok:"+rcv.isOk());
//            }
//        }

//      {
//        //0xB2(立即动作指令)
//        AgvEvent agvEvent = actionNow();
//        printInfo(agvEvent);
//        RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
//        if(rcv.isOk()){
//          System.out.println("0xB2 ok");
//        }else {
//          System.out.println();
//          System.out.println("0xB2 fail");
//          System.out.println("received transationId : "+ "isok:"+rcv.isOk());
//        }
//      }

//    {
//
//      System.out.println("00000");
//      Thread.sleep(100);
//      ActImmediately.reset();
//
//      //todo 读取动作执行状态
//      int execStatus = 2;
//      while (execStatus == 2) {
//        Thread.sleep(200);
//        byte[] value = ReadValue.command();
//        execStatus = value[0];
//      }
//
//      System.out.println("111111111");
//      Thread.sleep(100);
//      ActImmediately.reset();
//
//      //todo 读取动作执行状态
//      int execStatus2 = 2;
//      while (execStatus2 == 2) {
//        Thread.sleep(200);
//        byte[] value = ReadValue.command();
//        execStatus2 = value[0];
//      }
//
//      float height = 1.1f;
//      byte modeOfMovement = 0x01;
//
//      Thread.sleep(100);
//      //抬升叉齿指令
//      ActImmediately.command(height, modeOfMovement);
//
//      Thread.sleep(100);
//      ActImmediately.reset();
//    }
    {

      float height = 1.3f;
      byte modeOfMovement = 0x01;
      ActImmediately.allotsOperation(height, modeOfMovement);

//      //读取动作执行状态
//      ActImmediately.readActionState();
//      //重置
//      ActImmediately.reset();
//      //读取动作执行状态
//      ActImmediately.readActionState();
//      float height = 0.1f;
//      byte modeOfMovement = 0x02;
//      //执行叉齿指令
//      ActImmediately.command(height, modeOfMovement);
//      //读取动作执行状态
//      ActImmediately.readActionState();
//      //重置
//      ActImmediately.reset();
//      //读取重置状态
//      ActImmediately.readActionState();
//
//      ActImmediately.ACTION_EXECUTE_STATE = false;
    }

//    {
//            ActImmediately.reset();
//    }

//    {
//
//      //todo 读取动作执行状态
//      byte[] value = ReadValue.command();
//      int execStatus = value[0];
//
//      Thread.sleep(200);
//
//      while (execStatus == 2) {
//        byte[] value1 = ReadValue.command();
//        execStatus = value1[0];
//      }
//
//
//      ActImmediately.reset();
//
//      //todo 读取动作执行状态
//      byte[] value2 = ReadValue.command();
//      int execStatus2 = value2[0];
//
//      Thread.sleep(200);
//
//      while (execStatus2 == 2) {
//        byte[] value1 = ReadValue.command();
//        execStatus2 = value1[0];
//      }
//
//      float height = 0.1f;
//      byte modeOfMovement = 0x02;
//
//      ActImmediately.command(height, modeOfMovement);
//    }



  }


  public static String byteToHex(byte b) {
    // 将byte转换为无符号整数
    int unsignedByte = b & 0xFF;
    // 使用Integer.toHexString方法转换为十六进制字符串
    String hexString = Integer.toHexString(unsignedByte);
    // 如果字符串长度为1，需要在前面补0
    if (hexString.length() == 1) {
      return "0" + hexString;
    }
    return hexString;
  }

  public static void printInfo(AgvEvent agvEvent) {
    System.out.println("sended transationId : " + agvEvent.getTransationIdString());
    for (byte b : agvEvent.toBytes().getBody()) {
      System.out.print(byteToHex(b) + " ");
    }
  }

  public static void printInfo(RcvEventPackage rcv) {

    for (byte b : rcv.getDataBytes()) {
      System.out.print(byteToHex(b) + " ");
    }
  }

  /**
   * decs: read操作
   * 指令：0x01
   * author: caixiang
   * date: 2025/1/17 16:25
   */
  public static AgvEvent readValue() {
    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_READ);
    List<ReadValueMember> readValueMemberList = new ArrayList<>();
    ReadValueMember readValueMember1 = new ReadValueMember(Short.valueOf("10"), Short.valueOf("1"));
    readValueMemberList.add(readValueMember1);

    List<ReadStrValue> readStrValueList = new ArrayList<>();
    ReadStrValue readStrValue = new ReadStrValue("Lift", 1, readValueMemberList);
    readStrValueList.add(readStrValue);

    ReadParam readParam = new ReadParam(agvEvent.getTransationId(), readStrValueList);
    agvEvent.setBody(readParam.toBytes());

    return agvEvent;
  }

  /**
   * decs: write操作
   * 指令：0x03
   * author: caixiang
   * date: 2025/1/17 16:25
   */
  public static AgvEvent writeValue() {
    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_WRITE);

    List<WriteValueMember> valueMemberList = new ArrayList<>();
    WriteValueMember valueMember1 = new WriteValueMember(
        Short.valueOf("0"), Short.valueOf("4"), ByteUtils.uintToBytes(3, ByteOrder.LITTLE_ENDIAN)
    );
    valueMemberList.add(valueMember1);

    List<WriteStrValue> strValueList = new ArrayList<>();
    WriteStrValue strValue = new WriteStrValue("TestRW", 1, valueMemberList);
    strValueList.add(strValue);

    WriteParam param = new WriteParam(1, strValueList);
    agvEvent.setBody(param.toBytes());

    return agvEvent;
  }


  /**
   * decs: 查询机器人状态
   * 指令：0xAF
   * author: caixiang
   * date: 2025/1/17 16:25
   */
  public static AgvEvent queryStatus() {
    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_QUERY_ROBOT_STATUS);
    return agvEvent;
  }

  /**
   * decs: 立即动作指令
   * 指令：0xB2
   * author: caixiang
   * date: 2025/1/17 16:25
   */
  public static AgvEvent actionNow() {
    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ACT_IMMEDIATELY);
    //TODO 构建
    Integer actionID = 102;

//    //构建Action指令
//    Action actionLiftUp = new Action(
//        ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节
//        (byte) 0x02,   //只能执行当前动作
//        actionID,     //actionID
//        ActionSet.fork0x12_paramsize,
//        ActionSet.fork0x12(
//          1.1f,   //升降高度
//          (byte) 0x01,  //1上升  2下降
//            (byte)0x01   //01 开始 任务
//        )
//    );
//    agvEvent.setBody(actionLiftUp.toBytes());



//    //重置 空指令
        Action actionReset = new Action(
        ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节
        (byte) 0x02,   //只能执行当前动作
        actionID,     //actionID
        ActionSet.fork0x12_paramsize,
        ActionSet.fork0x12(
          0.0f,   //升降高度
          (byte) 0x01,  //1上升  2下降
            (byte)0x00   //01 开始 任务
        )
      );
      agvEvent.setBody(actionReset.toBytes());
//
//    //构建Action指令
//    Action actionLiftDown = new Action(
//        ActionSet.fork0x12, //机器人移动到正常点 停止，2个字节
//        (byte) 0x02,   //只能执行当前动作
//        actionID,     //actionID
//        ActionSet.fork0x12_paramsize,
//        ActionSet.fork0x12(
//            0.1f,   //升降高度
//            (byte) 0x02,  //1上升  2下降
//            (byte)0x01   //01 开始 任务
//        )
//    );
//    agvEvent.setBody(actionLiftDown.toBytes());
    return agvEvent;
  }

  /**
   * decs: 导航控制
   * 指令：0xAE
   * author: caixiang
   * date: 2025/1/17 16:25
   */
  public static AgvEvent navigationControl() {
    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_QUERY_ROBOT_STATUS);
    //TODO 构建
    Integer orderId = 1;

    //构建point
    Action[] pointActions1 = new Action[]{
        new Action(
            ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(
                orderId, (byte) 0x01
            )
        )
        //,new Action()...  每一个point 可以绑定一个或者多个 action
    };
    Action[] pointActions2 = new Action[]{
        new Action(
            ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(
                orderId, (byte) 0x01
            )
        )
    };
    Action[] pointActions3 = new Action[]{
        new Action(
            ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(
                orderId, (byte) 0x01
            )
        )
    };
    Point[] points = new Point[]{
        new Point(
            0, 1, 1f, (byte) 0x00, ByteUtils.usintTo1Byte(pointActions1.length), pointActions1
        ),
        new Point(
            2, 2, 1f, (byte) 0x00, ByteUtils.usintTo1Byte(pointActions2.length), pointActions2
        ),
        new Point(
            4, 3, 1f, (byte) 0x00, ByteUtils.usintTo1Byte(pointActions3.length), pointActions3
        )
    };

    //构建path
    Action[] pathActions1 = new Action[]{
        new Action(
            ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(
                orderId, (byte) 0x01
            )
        )
        //,new Action()...  每一个path 可以绑定一个或者多个 action
    };
    Action[] pathActions2 = new Action[]{
        new Action(
            ActionSet.stop0x01, (byte) 0x00, 1, ActionSet.stop0x01_paramsize, ActionSet.stop0x01(
                orderId, (byte) 0x01
            )
        )
    };
    Path[] paths = new Path[]{
        new Path(
            1, 1, 1f, (byte) 0x00, (byte) 0x00, ByteUtils.usintTo1Byte(pathActions1.length), 5f, 1f,
            pathActions1
        ),
        new Path(
            3, 2, 1f, (byte) 0x00, (byte) 0x00, ByteUtils.usintTo1Byte(pathActions2.length), 5f, 1f,
            pathActions2
        ),
    };
    NavigationParam navigationParam = new NavigationParam(
        1, 1, ByteUtils.usintTo1Byte(points.length), ByteUtils.usintTo1Byte(points.length - 1),
        points, paths
    );
    agvEvent.setBody(navigationParam.toBytes());

    return agvEvent;
  }

  /**
   * decs: 确认机器人位置
   * 指令：0x1F
   * author: caixiang
   * date: 2025/1/17 16:25
   */
  public static AgvEvent confirmInitialPosition() {
    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_CONFIRM_ROBOT_POSITION);
    return agvEvent;
  }

  /**
   * decs: 查询机器人运行状态
   * 指令：0x17
   * author: caixiang
   * date: 2025/1/17 16:25
   */
  public static AgvEvent queryRobotRunStatus() {
    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_QUERY_ROBOT_RUN_STATUS);
    return agvEvent;
  }

  public static AgvEvent manualLocation() {
    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ROBOT_SET_POSITION);
    RobotSetPosition robotSetPosition = new RobotSetPosition(11, 11, 11);
    byte[] bytes = robotSetPosition.toBytes();
    agvEvent.setBody(bytes);
    return agvEvent;
  }


  /**
   * decs: 查询载货状态
   * 指令：0xB0
   * author: caixiang
   * date: 2025/1/17 16:25
   */
  public static AgvEvent checkCargoStatus() {
    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_QUERY_CARRY_STATUS);
    return agvEvent;
  }

  /**
   * decs: 下发订阅信息
   * 指令：0xB1
   * author: caixiang
   * date: 2025/1/17 16:25
   */
  public static AgvEvent issueSubscribe() {
    List<SubscribeInfo> subscribeInfoList = new ArrayList<>();
    SubscribeInfo subscribeInfo = new SubscribeInfo(
        new byte[]{(byte) 0xaf, (byte) 0x00}, (short) 100, 1000
    );
    //SubscribeInfo subscribeInfo = new SubscribeInfo(new byte[]{(byte)0xb0,(byte)0x00}, (short) 100,1000);
    subscribeInfoList.add(subscribeInfo);
    SubscribeParam subscribeParam = new SubscribeParam(subscribeInfoList);
    AgvEvent agvEvent = new AgvEvent(
        AgvEventConstant.CommandCode_ISSUE_SUBSCRIPTION, subscribeParam.toBytes()
    );
    return agvEvent;
  }

}
